// 2025/4/8: 首个记录
// 2025/4/10： 修正GeneralElement数字判断
// 2025/6/12: 协议名常量改为public
// 2025/8/7: 新增ObjectClass.WheelBarrow, TruckHead, TrafficSign, TrafficLight

package com.appox.samples;

import com.appox.Sample;
import com.appox.GeneralSample;
import com.appox.GeneralElement;
import com.appox.Types.FloatPoint;
import java.util.ArrayList;
import java.util.List;
import java.awt.Color;

/**
 * 目标物传感器样本数据类
 */
public class ObjectSensorSample extends Sample {

    /**
     * 目标物分类枚举
     */
    public enum ObjectClass {
        GENERAL(1), // 一般物体大类
        CAR(2), // 车辆大类
        PEDESTRIAN(3), // 行人大类
        TRUCK(4), // 货车大类
        BIKE(5), // Bike大类
        RAIL_CAR(6), // 轨道车辆大类
        SPECIAL(7), // 特殊物体大类
        ROAD_STATIC(8), // 道路内静态物体大类
        SIDE_STATIC(9), // 道路外静态物体大类

        GENERAL_SMALL(11), // 一般小物体
        GENERAL_BIG(12), // 一般大物体
        BARRIER(13), // 一般障碍物

        VAN(21), // 面包车
        MINIBUS(22), // 小巴
        BUS(23), // 大巴
        BATTERY_CART(24), // 园区电瓶车
        TINY_CAR(25), // 微型车
        SUV(26), // SUV

        ADULT(31), // 成人
        CHILD(32), // 小孩
        SCOOTER(33), // 平衡车
        WHEEL_CHAIR(34), // 轮椅
        WHEEL_BARROW(35), // 手推车

        MINITRUCK(41), // 小卡车
        CONTAINER_TRUCK(42), // 货柜车（货柜车头 + 拖车）
        SPECIAL_CAR(43), // 特种车辆
        TRAILER(44), // 拖车
        TRUCK_HEAD(45), // 货柜车头

        MOTORBIKE(51), // 摩托车
        BICYCLE(52), // 自行车
        ELECTRIC_BIKE(53), // 电瓶自行车
        TRICYCLE(54), // 三轮车

        TRAIN(61), // 火车
        TRAM(62), // 有轨电车

        ANIMAL(71), // 动物
        BALL(72), // 球类
        LITTER(73), // 垃圾等杂物

        CONE(81), // 锥形路障
        MANHOLE_COVER(82), // 井盖
        PATCH(83), // 路面补丁
        GANTRY(84), // 龙门架

        POLE(91), // 竖杆
        TREE(92), // 树木
        VEGETATION(93), // 灌木
        BUILDING(94), // 建筑物
        TRAFFIC_SIGN(95), // 交通标志
        TRAFFIC_LIGHT(96); // 交通灯
        
        private final int value;
        
        ObjectClass(int value) {
            this.value = value;
        }
        
        public int getValue() {
            return value;
        }
        
        public static ObjectClass fromValue(int value) {
            for (ObjectClass cls : ObjectClass.values()) {
                if (cls.getValue() == value) {
                    return cls;
                }
            }
            return GENERAL;
        }
        
        /**
         * 获取目标物大类
         */
        public static ObjectClass getCategory(ObjectClass cls) {
            int id = cls.getValue();
            if (id < 10) return cls;
            return fromValue(id / 10);
        }
    }
    
    /**
     * 位置模式枚举
     */
    public enum PositionMode {
        CLOSEST_POINT(1), // 最近点
        BOX_CENTER(2); // 框中心
        
        private final int value;
        
        PositionMode(int value) {
            this.value = value;
        }
        
        public int getValue() {
            return value;
        }
        
        public static PositionMode fromValue(int value) {
            for (PositionMode mode : PositionMode.values()) {
                if (mode.getValue() == value) {
                    return mode;
                }
            }
            return CLOSEST_POINT;
        }
    }
    
    /**
     * 目标物信息类
     */
    public static class ObjectInfo {
        private int id; // 目标物ID
        private int age; // 目标物的Age
        private Integer rawId; // 目标物的原始ID
        private Integer rawAge; // 目标物的原始Age
        private Double confidence; // [%] 目标物的置信度
        
        private Integer timeOffset; // [us] 时间偏置(相对于帧的时间戳)
        private Color color; // 目标物的颜色
        
        private ObjectClass objectClass; // 目标物的类别
        private Integer objectRawClassId; // 目标物的原始类别ID
        private Double objectClassConfidence; // [%] 目标物的分类置信度
        
        private PositionMode posMode; // 目标物的位置模式
        private double posX; // [m] 目标物的x轴方向位置
        private double posY; // [m] 目标物的y轴方向位置
        private Double posZ; // [m] 目标物的z轴方向位置
        private Double posXSigma; // [m] 目标物x轴方向位置的精度
        private Double posYSigma; // [m] 目标物y轴方向位置的精度
        private Double posZSigma; // [m] 目标物z轴方向位置的精度
        
        private double cpx; // [m] 目标物的最近点x轴坐标
        private double cpy; // [m] 目标物的最近点y轴坐标
        private double cpd; // [m] 目标物的最近点与本车轮廓距离
        
        private Double vxAbs; // [KPH] x轴方向绝对速度
        private Double vyAbs; // [KPH] y轴方向绝对速度
        private Double vzAbs; // [KPH] z轴方向绝对速度
        private Double axAbs; // [m/s²] x轴方向绝对加速度
        private Double ayAbs; // [m/s²] y轴方向绝对加速度
        private Double azAbs; // [m/s²] z轴方向绝对加速度
        private Double vxRel; // [KPH] x轴方向相对速度
        private Double vyRel; // [KPH] y轴方向相对速度
        private Double vzRel; // [KPH] z轴方向相对速度
        private Double axRel; // [m/s²] x轴方向相对加速度
        private Double ayRel; // [m/s²] y轴方向相对加速度
        private Double azRel; // [m/s²] z轴方向相对加速度
        
        private Double yawRate; // [deg/s] 横摆角速度
        private Double curvature; // [1/m] 转弯曲率
        
        private Double heading; // [deg] 目标物的朝向
        private Double width; // [m] 目标物的宽度
        private Double length; // [m] 目标物的长度
        private Double height; // [m] 目标物的高度
        private FloatPoint[] trajectory; // 目标物轨迹点的列表
        private FloatPoint[] contour; // 目标物轮廓点的列表
        
        private Double rcs; // [dB] 雷达目标物的雷达散射截面
        private Double vLng; // [m/s] 雷达目标物的径向速度
        private Double vLat; // [m/s] 雷达目标物的切向速度
        
        public ObjectInfo() {
            id = 0;
            age = 0;
            posX = 0;
            posY = 0;
            posMode = PositionMode.CLOSEST_POINT;
            cpx = 0;
            cpy = 0;
            cpd = 0;
            objectClass = ObjectClass.GENERAL;
        }

        /**
         * 获取目标物位置
         */
        public FloatPoint getObjectPosition() {
            FloatPoint pt = new FloatPoint();
            pt.setX((float)posX);
            pt.setY((float)posY);
            return pt;
        }
        
        /**
         * 获取目标物最近点
         */
        public FloatPoint getClosestPoint() {
            FloatPoint pt = new FloatPoint();
            pt.setX((float)cpx);
            pt.setY((float)cpy);
            return pt;
        }
        
        /**
         * 获取目标物速度(绝对)
         */
        public Double getSpeed() {
            if (vxAbs != null && vyAbs != null) {
                return Math.sqrt(vxAbs * vxAbs + vyAbs * vyAbs);
            }
            return null;
        }
        
        /**
         * 获取目标物大类
         */
        public ObjectClass getObjectCategory() {
            return ObjectClass.getCategory(objectClass);
        }
        
        public int getId() {
            return id;
        }
        
        public void setId(int id) {
            this.id = id;
        }
        
        public int getAge() {
            return age;
        }
        
        public void setAge(int age) {
            this.age = age;
        }
        
        public Integer getRawId() {
            return rawId;
        }
        
        public void setRawId(Integer rawId) {
            this.rawId = rawId;
        }
        
        public Integer getRawAge() {
            return rawAge;
        }
        
        public void setRawAge(Integer rawAge) {
            this.rawAge = rawAge;
        }
        
        public Double getConfidence() {
            return confidence;
        }
        
        public void setConfidence(Double confidence) {
            this.confidence = confidence;
        }
        
        public Integer getTimeOffset() {
            return timeOffset;
        }
        
        public void setTimeOffset(Integer timeOffset) {
            this.timeOffset = timeOffset;
        }
        
        public Color getColor() {
            return color;
        }
        
        public void setColor(Color color) {
            this.color = color;
        }
        
        public ObjectClass getObjectClass() {
            return objectClass;
        }
        
        public void setObjectClass(ObjectClass objectClass) {
            this.objectClass = objectClass;
        }
        
        public Integer getObjectRawClassId() {
            return objectRawClassId;
        }
        
        public void setObjectRawClassId(Integer objectRawClassId) {
            this.objectRawClassId = objectRawClassId;
        }
        
        public Double getObjectClassConfidence() {
            return objectClassConfidence;
        }
        
        public void setObjectClassConfidence(Double objectClassConfidence) {
            this.objectClassConfidence = objectClassConfidence;
        }
        
        public PositionMode getPosMode() {
            return posMode;
        }
        
        public void setPosMode(PositionMode posMode) {
            this.posMode = posMode;
        }
        
        public double getPosX() {
            return posX;
        }
        
        public void setPosX(double posX) {
            this.posX = posX;
        }
        
        public double getPosY() {
            return posY;
        }
        
        public void setPosY(double posY) {
            this.posY = posY;
        }
        
        public Double getPosZ() {
            return posZ;
        }
        
        public void setPosZ(Double posZ) {
            this.posZ = posZ;
        }
        
        public Double getPosXSigma() {
            return posXSigma;
        }
        
        public void setPosXSigma(Double posXSigma) {
            this.posXSigma = posXSigma;
        }
        
        public Double getPosYSigma() {
            return posYSigma;
        }
        
        public void setPosYSigma(Double posYSigma) {
            this.posYSigma = posYSigma;
        }
        
        public Double getPosZSigma() {
            return posZSigma;
        }
        
        public void setPosZSigma(Double posZSigma) {
            this.posZSigma = posZSigma;
        }
        
        public double getCpx() {
            return cpx;
        }
        
        public void setCpx(double cpx) {
            this.cpx = cpx;
        }
        
        public double getCpy() {
            return cpy;
        }
        
        public void setCpy(double cpy) {
            this.cpy = cpy;
        }
        
        public double getCpd() {
            return cpd;
        }
        
        public void setCpd(double cpd) {
            this.cpd = cpd;
        }
        
        public Double getVxAbs() {
            return vxAbs;
        }
        
        public void setVxAbs(Double vxAbs) {
            this.vxAbs = vxAbs;
        }
        
        public Double getVyAbs() {
            return vyAbs;
        }
        
        public void setVyAbs(Double vyAbs) {
            this.vyAbs = vyAbs;
        }
        
        public Double getVzAbs() {
            return vzAbs;
        }
        
        public void setVzAbs(Double vzAbs) {
            this.vzAbs = vzAbs;
        }
        
        public Double getAxAbs() {
            return axAbs;
        }
        
        public void setAxAbs(Double axAbs) {
            this.axAbs = axAbs;
        }
        
        public Double getAyAbs() {
            return ayAbs;
        }
        
        public void setAyAbs(Double ayAbs) {
            this.ayAbs = ayAbs;
        }
        
        public Double getAzAbs() {
            return azAbs;
        }
        
        public void setAzAbs(Double azAbs) {
            this.azAbs = azAbs;
        }
        
        public Double getVxRel() {
            return vxRel;
        }
        
        public void setVxRel(Double vxRel) {
            this.vxRel = vxRel;
        }
        
        public Double getVyRel() {
            return vyRel;
        }
        
        public void setVyRel(Double vyRel) {
            this.vyRel = vyRel;
        }
        
        public Double getVzRel() {
            return vzRel;
        }
        
        public void setVzRel(Double vzRel) {
            this.vzRel = vzRel;
        }
        
        public Double getAxRel() {
            return axRel;
        }
        
        public void setAxRel(Double axRel) {
            this.axRel = axRel;
        }
        
        public Double getAyRel() {
            return ayRel;
        }
        
        public void setAyRel(Double ayRel) {
            this.ayRel = ayRel;
        }
        
        public Double getAzRel() {
            return azRel;
        }
        
        public void setAzRel(Double azRel) {
            this.azRel = azRel;
        }
        
        public Double getYawRate() {
            return yawRate;
        }
        
        public void setYawRate(Double yawRate) {
            this.yawRate = yawRate;
        }
        
        public Double getCurvature() {
            return curvature;
        }
        
        public void setCurvature(Double curvature) {
            this.curvature = curvature;
        }
        
        public Double getHeading() {
            return heading;
        }
        
        public void setHeading(Double heading) {
            this.heading = heading;
        }
        
        public Double getWidth() {
            return width;
        }
        
        public void setWidth(Double width) {
            this.width = width;
        }
        
        public Double getLength() {
            return length;
        }
        
        public void setLength(Double length) {
            this.length = length;
        }
        
        public Double getHeight() {
            return height;
        }
        
        public void setHeight(Double height) {
            this.height = height;
        }
        
        public FloatPoint[] getTrajectory() {
            return trajectory;
        }
        
        public void setTrajectory(FloatPoint[] trajectory) {
            this.trajectory = trajectory;
        }
        
        public FloatPoint[] getContour() {
            return contour;
        }
        
        public void setContour(FloatPoint[] contour) {
            this.contour = contour;
        }
        
        public Double getRcs() {
            return rcs;
        }
        
        public void setRcs(Double rcs) {
            this.rcs = rcs;
        }
        
        public Double getVLng() {
            return vLng;
        }
        
        public void setVLng(Double vLng) {
            this.vLng = vLng;
        }
        
        public Double getVLat() {
            return vLat;
        }
        
        public void setVLat(Double vLat) {
            this.vLat = vLat;
        }
    }
    
    /**
     * FOV描述类
     */
    public static class FovDescription {
        private double positionX; // [m] FOV中心点x轴坐标
        private double positionY; // [m] FOV中心的y轴坐标
        private double angleRange; // [deg] FOV的角度范围
        private double orientation; // [deg] FOV中轴线朝向角
        private double distanceRange; // [m] FOV探测距离范围
        private double blindRange; // [m] FOV盲区范围
        
        public FovDescription() {
            positionX = 0;
            positionY = 0;
            angleRange = 0;
            orientation = 0;
            distanceRange = 0;
            blindRange = 0;
        }
        
        // Getters and Setters
        public double getPositionX() {
            return positionX;
        }
        
        public void setPositionX(double positionX) {
            this.positionX = positionX;
        }
        
        public double getPositionY() {
            return positionY;
        }
        
        public void setPositionY(double positionY) {
            this.positionY = positionY;
        }
        
        public double getAngleRange() {
            return angleRange;
        }
        
        public void setAngleRange(double angleRange) {
            this.angleRange = angleRange;
        }
        
        public double getOrientation() {
            return orientation;
        }
        
        public void setOrientation(double orientation) {
            this.orientation = orientation;
        }
        
        public double getDistanceRange() {
            return distanceRange;
        }
        
        public void setDistanceRange(double distanceRange) {
            this.distanceRange = distanceRange;
        }
        
        public double getBlindRange() {
            return blindRange;
        }
        
        public void setBlindRange(double blindRange) {
            this.blindRange = blindRange;
        }
    }
    
    // 目标物及关键目标
    private List<ObjectInfo> objects; // 目标物列表
    private Integer keyObjectFrontIndex; // 前向关键目标序号
    private Integer keyObjectLeftIndex; // 左侧关键目标序号
    private Integer keyObjectRightIndex; // 右侧关键目标序号
    
    // 车辆参数
    private Double vehicleSpeed; // [KPH] 本车车速
    private Double vehicleCurvature; // [1/m] 本车曲率
    private Double vehicleWidth; // [m] 本车宽度
    private Double vehicleLength; // [m] 本车长度
    private Double vehicleWheelBase; // [m] 本车轴距
    private Double vehicleFrontOverhang; // [m] 本车前悬
    
    // FOV列表
    private List<FovDescription> fovs; // FOV列表

    /**
     * 构造函数
     */
    public ObjectSensorSample() {
        objects = new ArrayList<>();
        fovs = new ArrayList<>();
    }
    
    /**
     * 获取目标物列表
     */
    public List<ObjectInfo> getObjects() {
        return objects;
    }
    
    /**
     * 设置目标物列表
     */
    public void setObjects(List<ObjectInfo> objects) {
        this.objects = objects;
    }
    
    /**
     * 获取前向关键目标序号
     */
    public Integer getKeyObjectFrontIndex() {
        return keyObjectFrontIndex;
    }
    
    /**
     * 设置前向关键目标序号
     */
    public void setKeyObjectFrontIndex(Integer keyObjectFrontIndex) {
        this.keyObjectFrontIndex = keyObjectFrontIndex;
    }
    
    /**
     * 获取左侧关键目标序号
     */
    public Integer getKeyObjectLeftIndex() {
        return keyObjectLeftIndex;
    }
    
    /**
     * 设置左侧关键目标序号
     */
    public void setKeyObjectLeftIndex(Integer keyObjectLeftIndex) {
        this.keyObjectLeftIndex = keyObjectLeftIndex;
    }
    
    /**
     * 获取右侧关键目标序号
     */
    public Integer getKeyObjectRightIndex() {
        return keyObjectRightIndex;
    }
    
    /**
     * 设置右侧关键目标序号
     */
    public void setKeyObjectRightIndex(Integer keyObjectRightIndex) {
        this.keyObjectRightIndex = keyObjectRightIndex;
    }
    
    /**
     * 获取本车车速
     */
    public Double getVehicleSpeed() {
        return vehicleSpeed;
    }
    
    /**
     * 设置本车车速
     */
    public void setVehicleSpeed(Double vehicleSpeed) {
        this.vehicleSpeed = vehicleSpeed;
    }
    
    /**
     * 获取本车曲率
     */
    public Double getVehicleCurvature() {
        return vehicleCurvature;
    }
    
    /**
     * 设置本车曲率
     */
    public void setVehicleCurvature(Double vehicleCurvature) {
        this.vehicleCurvature = vehicleCurvature;
    }
    
    /**
     * 获取本车宽度
     */
    public Double getVehicleWidth() {
        return vehicleWidth;
    }
    
    /**
     * 设置本车宽度
     */
    public void setVehicleWidth(Double vehicleWidth) {
        this.vehicleWidth = vehicleWidth;
    }
    
    /**
     * 获取本车长度
     */
    public Double getVehicleLength() {
        return vehicleLength;
    }
    
    /**
     * 设置本车长度
     */
    public void setVehicleLength(Double vehicleLength) {
        this.vehicleLength = vehicleLength;
    }
    
    /**
     * 获取本车轴距
     */
    public Double getVehicleWheelBase() {
        return vehicleWheelBase;
    }
    
    /**
     * 设置本车轴距
     */
    public void setVehicleWheelBase(Double vehicleWheelBase) {
        this.vehicleWheelBase = vehicleWheelBase;
    }
    
    /**
     * 获取本车前悬
     */
    public Double getVehicleFrontOverhang() {
        return vehicleFrontOverhang;
    }
    
    /**
     * 设置本车前悬
     */
    public void setVehicleFrontOverhang(Double vehicleFrontOverhang) {
        this.vehicleFrontOverhang = vehicleFrontOverhang;
    }
    
    /**
     * 获取FOV列表
     */
    public List<FovDescription> getFovs() {
        return fovs;
    }
    
    /**
     * 设置FOV列表
     */
    public void setFovs(List<FovDescription> fovs) {
        this.fovs = fovs;
    }
    
    /**
     * 获取通用样本协议
     */
    @Override
    public String getGeneralSampleProtocol() {
        return TARGET_PROTOCOL;
    }
    
    /**
     * 获取通用样本协议列表
     */
    @Override
    public String[] getGeneralSampleProtocols() {
        return PROTOCOLS;
    }
    
    /**
     * 将目标物传感器样本转换为通用样本
     */
    public GeneralSample toGeneralSample(int channel) {
        GeneralSample sample = new GeneralSample();
        sample.setTime(this);
        sample.setProtocol(TARGET_PROTOCOL);
        sample.setChannel(channel);

        int trajectoryPoints = 0;
        int contourPoints = 0;
        int[] trajectoryOffset = new int[objects.size()];
        int[] trajectorySize = new int[objects.size()];
        int[] contourOffset = new int[objects.size()];
        int[] contourSize = new int[objects.size()];
        
        for (int i = 0; i < objects.size(); i++) {
            trajectoryOffset[i] = trajectoryPoints;
            trajectorySize[i] = objects.get(i).getTrajectory() == null ? 0 : objects.get(i).getTrajectory().length;
            trajectoryPoints += trajectorySize[i];
            contourOffset[i] = contourPoints;
            contourSize[i] = objects.get(i).getContour() == null ? 0 : objects.get(i).getContour().length;
            contourPoints += contourSize[i];
        }

        GeneralElement[] data = new GeneralElement[20 + objects.size() * 72 + fovs.size() * 6 + (trajectoryPoints + contourPoints) * 2];
        
        // 初始化所有元素
        for (int i = 0; i < data.length; i++) {
            data[i] = new GeneralElement();
        }

        int objBase = 20;
        int fovBase = objBase + objects.size() * 72;
        int trajectoryBase = fovBase + fovs.size() * 6;
        int contourBase = trajectoryBase + trajectoryPoints * 2;

        data[0] = new GeneralElement((double)objects.size());
        data[1] = new GeneralElement((double)fovs.size());
        data[2] = new GeneralElement((double)trajectoryPoints);
        data[3] = new GeneralElement((double)contourPoints);

        if (keyObjectFrontIndex != null) data[4] = new GeneralElement((double)keyObjectFrontIndex);
        if (keyObjectLeftIndex != null) data[5] = new GeneralElement((double)keyObjectLeftIndex);
        if (keyObjectRightIndex != null) data[6] = new GeneralElement((double)keyObjectRightIndex);

        if (vehicleSpeed != null) data[7] = new GeneralElement(vehicleSpeed);
        if (vehicleCurvature != null) data[8] = new GeneralElement(vehicleCurvature);
        if (vehicleWidth != null) data[9] = new GeneralElement(vehicleWidth);
        if (vehicleLength != null) data[10] = new GeneralElement(vehicleLength);
        if (vehicleWheelBase != null) data[11] = new GeneralElement(vehicleWheelBase);
        if (vehicleFrontOverhang != null) data[12] = new GeneralElement(vehicleFrontOverhang);

        for (int i = 0; i < objects.size(); i++) {
            ObjectInfo src = objects.get(i);
            int baseIndex = objBase + 72 * i;
            
            data[baseIndex + 0] = new GeneralElement((double)src.getId());
            data[baseIndex + 1] = new GeneralElement((double)src.getAge());
            if (src.getRawId() != null) data[baseIndex + 2] = new GeneralElement((double)src.getRawId());
            if (src.getRawAge() != null) data[baseIndex + 3] = new GeneralElement((double)src.getRawAge());
            if (src.getObjectRawClassId() != null) data[baseIndex + 4] = new GeneralElement((double)src.getObjectRawClassId());
            data[baseIndex + 5] = new GeneralElement((double)src.getObjectClass().getValue());
            data[baseIndex + 6] = new GeneralElement((double)src.getPosMode().getValue());
            data[baseIndex + 7] = new GeneralElement(src.getPosX());
            data[baseIndex + 8] = new GeneralElement(src.getPosY());
            if (src.getPosZ() != null) data[baseIndex + 9] = new GeneralElement(src.getPosZ());
            if (src.getPosXSigma() != null) data[baseIndex + 10] = new GeneralElement(src.getPosXSigma());
            if (src.getPosYSigma() != null) data[baseIndex + 11] = new GeneralElement(src.getPosYSigma());
            if (src.getPosZSigma() != null) data[baseIndex + 12] = new GeneralElement(src.getPosZSigma());
            data[baseIndex + 13] = new GeneralElement(src.getCpx());
            data[baseIndex + 14] = new GeneralElement(src.getCpy());
            data[baseIndex + 15] = new GeneralElement(src.getCpd());
            if (src.getWidth() != null) data[baseIndex + 16] = new GeneralElement(src.getWidth());
            if (src.getLength() != null) data[baseIndex + 17] = new GeneralElement(src.getLength());
            if (src.getHeight() != null) data[baseIndex + 18] = new GeneralElement(src.getHeight());
            if (src.getHeading() != null) data[baseIndex + 19] = new GeneralElement(src.getHeading());
            if (src.getVxRel() != null) data[baseIndex + 20] = new GeneralElement(src.getVxRel());
            if (src.getVxAbs() != null) data[baseIndex + 21] = new GeneralElement(src.getVxAbs());
            if (src.getVyRel() != null) data[baseIndex + 22] = new GeneralElement(src.getVyRel());
            if (src.getVyAbs() != null) data[baseIndex + 23] = new GeneralElement(src.getVyAbs());
            if (src.getVzRel() != null) data[baseIndex + 24] = new GeneralElement(src.getVzRel());
            if (src.getVzAbs() != null) data[baseIndex + 25] = new GeneralElement(src.getVzAbs());
            if (src.getAxRel() != null) data[baseIndex + 26] = new GeneralElement(src.getAxRel());
            if (src.getAxAbs() != null) data[baseIndex + 27] = new GeneralElement(src.getAxAbs());
            if (src.getAyRel() != null) data[baseIndex + 28] = new GeneralElement(src.getAyRel());
            if (src.getAyAbs() != null) data[baseIndex + 29] = new GeneralElement(src.getAyAbs());
            if (src.getAzRel() != null) data[baseIndex + 30] = new GeneralElement(src.getAzRel());
            if (src.getAzAbs() != null) data[baseIndex + 31] = new GeneralElement(src.getAzAbs());
            
            if (src.getColor() != null) {
                data[baseIndex + 32] = new GeneralElement((double)src.getColor().getRed());
                data[baseIndex + 33] = new GeneralElement((double)src.getColor().getGreen());
                data[baseIndex + 34] = new GeneralElement((double)src.getColor().getBlue());
            }
            
            if (src.getTimeOffset() != null) data[baseIndex + 35] = new GeneralElement((double)src.getTimeOffset());
            if (src.getConfidence() != null) data[baseIndex + 36] = new GeneralElement(src.getConfidence());
            if (src.getObjectClassConfidence() != null) data[baseIndex + 37] = new GeneralElement(src.getObjectClassConfidence());
            if (src.getRcs() != null) data[baseIndex + 46] = new GeneralElement(src.getRcs());
            if (src.getVLng() != null) data[baseIndex + 47] = new GeneralElement(src.getVLng());
            if (src.getVLat() != null) data[baseIndex + 48] = new GeneralElement(src.getVLat());
            if (src.getYawRate() != null) data[baseIndex + 49] = new GeneralElement(src.getYawRate());
            if (src.getCurvature() != null) data[baseIndex + 50] = new GeneralElement(src.getCurvature());

            Double ttc1 = getTTC1(src);
            if (ttc1 != null) data[baseIndex + 38] = new GeneralElement(ttc1);
            Double ttc2 = getTTC2(src);
            if (ttc2 != null) data[baseIndex + 39] = new GeneralElement(ttc2);
            Double thw = getTHW(src);
            if (thw != null) data[baseIndex + 40] = new GeneralElement(thw);
            Double pdca = getDCA(src, 1.0);
            if (pdca != null) data[baseIndex + 41] = new GeneralElement(pdca);
            Double odca = getDCA(src, 0.0);
            if (odca != null) data[baseIndex + 42] = new GeneralElement(odca);
            Double overlap = getOverlap(src, 0.98);
            if (overlap != null) data[baseIndex + 43] = new GeneralElement(overlap);
            Double lc = getLC(src, 0.98);
            if (lc != null) data[baseIndex + 44] = new GeneralElement(lc);
            Double rc = getRC(src, 0.98);
            if (rc != null) data[baseIndex + 45] = new GeneralElement(rc);

            Double speed = src.getSpeed();
            if (speed != null) data[baseIndex + 51] = new GeneralElement(speed);

            data[baseIndex + 68] = new GeneralElement((double)trajectoryOffset[i]);
            data[baseIndex + 69] = new GeneralElement((double)trajectorySize[i]);
            data[baseIndex + 70] = new GeneralElement((double)contourOffset[i]);
            data[baseIndex + 71] = new GeneralElement((double)contourSize[i]);

            int dataOffset = trajectoryBase + trajectoryOffset[i] * 2;
            for (int n = 0; n < trajectorySize[i]; n++) {
                FloatPoint pt = src.getTrajectory()[n];
                data[dataOffset + 2 * n] = new GeneralElement((double)pt.getX());
                data[dataOffset + 2 * n + 1] = new GeneralElement((double)pt.getY());
            }

            dataOffset = contourBase + contourOffset[i] * 2;
            for (int n = 0; n < contourSize[i]; n++) {
                FloatPoint pt = src.getContour()[n];
                data[dataOffset + 2 * n] = new GeneralElement((double)pt.getX());
                data[dataOffset + 2 * n + 1] = new GeneralElement((double)pt.getY());
            }
        }

        for (int i = 0; i < fovs.size(); i++) {
            FovDescription src = fovs.get(i);
            int baseIndex = fovBase + 6 * i;
            data[baseIndex + 0] = new GeneralElement(src.getPositionX());
            data[baseIndex + 1] = new GeneralElement(src.getPositionY());
            data[baseIndex + 2] = new GeneralElement(src.getOrientation());
            data[baseIndex + 3] = new GeneralElement(src.getAngleRange());
            data[baseIndex + 4] = new GeneralElement(src.getDistanceRange());
            data[baseIndex + 5] = new GeneralElement(src.getBlindRange());
        }

        sample.setValues(data);
        sample.setNumberOfSignificants(trajectoryBase);

        return sample;
    }
    
    /**
     * 从通用样本恢复目标物传感器样本
     */
    @Override
    public boolean fromGeneralSample(GeneralSample sample) {
        setTime(sample);
        
        String protocol = sample.getProtocol();
        if ("obj-sensor-sample-v7".equals(protocol)) {
            return fromObjSensorSampleV7(sample);
        } else if ("obj-sensor-sample-v6".equals(protocol)) {
            return fromObjSensorSampleV6(sample);
        } else if ("obj-sensor-sample-v5".equals(protocol)) {
            return fromObjSensorSampleV5(sample);
        } else if ("obj-sensor-sample-v4".equals(protocol)) {
            return fromObjSensorSampleV4(sample);
        } else if ("obj-sensor-sample-v3".equals(protocol)) {
            return fromObjSensorSampleV3(sample);
        }
        return false;
    }

    private boolean fromObjSensorSampleV3(GeneralSample sample) {
        GeneralElement[] v = sample.getValues();
        if (v.length < 7) return false;
        if (isNotNumber(v[0]) || v[0].getValue() != sample.getChannel()) return false;

        if (isNotNumber(v[1]) ||
            isNotNumber(v[2]) ||
            isNotNumber(v[3])) return false;

        int nObjects = (int)v[1].getValue();
        int nTrajectories = (int)v[2].getValue();
        int nContours = (int)v[3].getValue();
        if (v.length != 7 + nObjects * 33 + (nTrajectories + nContours) * 2) return false;

        keyObjectFrontIndex = toInt(v[4]);
        keyObjectLeftIndex = toInt(v[5]);
        keyObjectRightIndex = toInt(v[6]);

        int trajBase = 7 + 33 * nObjects;
        int contBase = 7 + 33 * nObjects + 2 * nTrajectories;
        objects.clear();
        for (int i = 0; i < nObjects; i++) {
            ObjectInfo obj = new ObjectInfo();
            obj.setId((int)v[33 * i + 7].getValue());
            obj.setAge((int)v[33 * i + 8].getValue());
            obj.setObjectRawClassId(toInt(v[33 * i + 9]));
            obj.setObjectClass(ObjectClass.fromValue((int)v[33 * i + 10].getValue()));
            obj.setPosMode(PositionMode.fromValue((int)v[33 * i + 11].getValue()));
            obj.setPosX(v[33 * i + 12].getValue());
            obj.setPosY(v[33 * i + 13].getValue());
            obj.setCpx(v[33 * i + 14].getValue());
            obj.setCpy(v[33 * i + 15].getValue());
            obj.setWidth(toDouble(v[33 * i + 17]));
            obj.setLength(toDouble(v[33 * i + 18]));
            obj.setHeading(toDouble(v[33 * i + 19]));
            obj.setVxRel(toDouble(v[33 * i + 20]));
            obj.setVxAbs(toDouble(v[33 * i + 21]));
            obj.setVyRel(toDouble(v[33 * i + 22]));
            obj.setVyAbs(toDouble(v[33 * i + 23]));
            obj.setAxRel(toDouble(v[33 * i + 24]));
            obj.setAxAbs(toDouble(v[33 * i + 25]));
            obj.setAyRel(toDouble(v[33 * i + 26]));
            obj.setAyAbs(toDouble(v[33 * i + 27]));

            int trajOffset = (int)v[33 * i + 36].getValue();
            int trajSize = (int)v[33 * i + 37].getValue();
            int contOffset = (int)v[33 * i + 38].getValue();
            int contSize = (int)v[33 * i + 39].getValue();

            FloatPoint[] trajectory = new FloatPoint[trajSize];
            for (int n = 0; n < trajSize; n++) {
                trajectory[n] = new FloatPoint();
                trajectory[n].setX((float)v[trajBase + 2 * trajOffset + 2 * n].getValue());
                trajectory[n].setY((float)v[trajBase + 2 * trajOffset + 2 * n + 1].getValue());
            }
            obj.setTrajectory(trajectory);

            FloatPoint[] contour = new FloatPoint[contSize];
            for (int n = 0; n < contSize; n++) {
                contour[n] = new FloatPoint();
                contour[n].setX((float)v[contBase + 2 * contOffset + 2 * n].getValue());
                contour[n].setY((float)v[contBase + 2 * contOffset + 2 * n + 1].getValue());
            }
            obj.setContour(contour);

            objects.add(obj);
        }

        return true;
    }

    private boolean fromObjSensorSampleV4(GeneralSample sample) {
        GeneralElement[] v = sample.getValues();
        if (v.length < 16) return false;

        if (isNotNumber(v[0]) ||
            isNotNumber(v[1]) ||
            isNotNumber(v[2])) return false;

        int nObjects = (int)v[0].getValue();
        int nTrajectories = (int)v[1].getValue();
        int nContours = (int)v[2].getValue();
        if (v.length != 16 + nObjects * 35 + (nTrajectories + nContours) * 2) return false;

        keyObjectFrontIndex = toInt(v[3]);
        keyObjectLeftIndex = toInt(v[4]);
        keyObjectRightIndex = toInt(v[5]);

        vehicleSpeed = toDouble(v[6]);
        vehicleCurvature = toDouble(v[7]);
        vehicleWidth = toDouble(v[8]);
        vehicleLength = toDouble(v[9]);

        double fovPosX = v[10].getValue();
        double fovPosY = v[11].getValue();
        double fovOrient = v[12].getValue();
        double fovAngleRange = v[13].getValue();
        double fovDistRange = v[14].getValue();
        boolean fovSymmetry = v[15].getValue() == 1;

        fovs.clear();
        FovDescription fov1 = new FovDescription();
        fov1.setPositionX(fovPosX);
        fov1.setPositionY(fovPosY);
        fov1.setOrientation(fovOrient);
        fov1.setAngleRange(fovAngleRange);
        fov1.setDistanceRange(fovDistRange);
        fovs.add(fov1);
        
        if (fovSymmetry) {
            FovDescription fov2 = new FovDescription();
            fov2.setPositionX(fovPosX);
            fov2.setPositionY(-fovPosY);
            fov2.setOrientation(-fovOrient);
            fov2.setAngleRange(fovAngleRange);
            fov2.setDistanceRange(fovDistRange);
            fovs.add(fov2);
        }

        int trajBase = 16 + 35 * nObjects;
        int contBase = 16 + 35 * nObjects + 2 * nTrajectories;
        objects.clear();
        for (int i = 0; i < nObjects; i++) {
            ObjectInfo obj = new ObjectInfo();
            int baseIndex = 16 + 35 * i;
            obj.setId((int)v[baseIndex + 0].getValue());
            obj.setAge((int)v[baseIndex + 1].getValue());
            obj.setRawId(toInt(v[baseIndex + 2]));
            obj.setRawAge(toInt(v[baseIndex + 3]));
            obj.setObjectRawClassId(toInt(v[baseIndex + 4]));
            obj.setObjectClass(ObjectClass.fromValue((int)v[baseIndex + 5].getValue()));
            obj.setPosMode(PositionMode.fromValue((int)v[baseIndex + 6].getValue()));
            obj.setPosX(v[baseIndex + 7].getValue());
            obj.setPosY(v[baseIndex + 8].getValue());
            obj.setCpx(v[baseIndex + 9].getValue());
            obj.setCpy(v[baseIndex + 10].getValue());
            obj.setCpd(v[baseIndex + 11].getValue());
            obj.setWidth(toDouble(v[baseIndex + 12]));
            obj.setLength(toDouble(v[baseIndex + 13]));
            obj.setHeading(toDouble(v[baseIndex + 14]));
            obj.setVxRel(toDouble(v[baseIndex + 15]));
            obj.setVxAbs(toDouble(v[baseIndex + 16]));
            obj.setVyRel(toDouble(v[baseIndex + 17]));
            obj.setVyAbs(toDouble(v[baseIndex + 18]));
            obj.setAxRel(toDouble(v[baseIndex + 19]));
            obj.setAxAbs(toDouble(v[baseIndex + 20]));
            obj.setAyRel(toDouble(v[baseIndex + 21]));
            obj.setAyAbs(toDouble(v[baseIndex + 22]));

            int trajOffset = (int)v[baseIndex + 31].getValue();
            int trajSize = (int)v[baseIndex + 32].getValue();
            int contOffset = (int)v[baseIndex + 33].getValue();
            int contSize = (int)v[baseIndex + 34].getValue();

            FloatPoint[] trajectory = new FloatPoint[trajSize];
            for (int n = 0; n < trajSize; n++) {
                trajectory[n] = new FloatPoint();
                trajectory[n].setX((float)v[trajBase + 2 * trajOffset + 2 * n].getValue());
                trajectory[n].setY((float)v[trajBase + 2 * trajOffset + 2 * n + 1].getValue());
            }
            obj.setTrajectory(trajectory);

            FloatPoint[] contour = new FloatPoint[contSize];
            for (int n = 0; n < contSize; n++) {
                contour[n] = new FloatPoint();
                contour[n].setX((float)v[contBase + 2 * contOffset + 2 * n].getValue());
                contour[n].setY((float)v[contBase + 2 * contOffset + 2 * n + 1].getValue());
            }
            obj.setContour(contour);

            objects.add(obj);
        }

        return true;
    }

    private boolean fromObjSensorSampleV5(GeneralSample sample) {
        GeneralElement[] v = sample.getValues();
        if (v.length < 11) return false;

        if (isNotNumber(v[0]) ||
            isNotNumber(v[1]) ||
            isNotNumber(v[2])) return false;

        int nObjects = (int)v[0].getValue();
        int nTrajectories = (int)v[1].getValue();
        int nContours = (int)v[2].getValue();
        int nFovs = (int)v[10].getValue();

        int sizeWithExtra = 11 + nObjects * 42 + nFovs * 6 + (nTrajectories + nContours) * 2;
        int sizeWithoutExtra = 11 + nObjects * 42 + nFovs * 6;
        if (v.length != sizeWithExtra && v.length != sizeWithoutExtra) return false;

        keyObjectFrontIndex = toInt(v[3]);
        keyObjectLeftIndex = toInt(v[4]);
        keyObjectRightIndex = toInt(v[5]);

        vehicleSpeed = toDouble(v[6]);
        vehicleCurvature = toDouble(v[7]);
        vehicleWidth = toDouble(v[8]);
        vehicleLength = toDouble(v[9]);

        int objBase = 11;
        int fovBase = objBase + 42 * nObjects;
        int trajBase = fovBase + 6 * nFovs;
        int contBase = trajBase + 2 * nTrajectories;

        objects.clear();
        for (int i = 0; i < nObjects; i++) {
            ObjectInfo obj = new ObjectInfo();
            int baseIndex = objBase + 42 * i;
            obj.setId((int)v[baseIndex + 0].getValue());
            obj.setAge((int)v[baseIndex + 1].getValue());
            obj.setRawId(toInt(v[baseIndex + 2]));
            obj.setRawAge(toInt(v[baseIndex + 3]));
            obj.setObjectRawClassId(toInt(v[baseIndex + 4]));
            obj.setObjectClass(ObjectClass.fromValue((int)v[baseIndex + 5].getValue()));
            obj.setPosMode(PositionMode.fromValue((int)v[baseIndex + 6].getValue()));
            obj.setPosX(v[baseIndex + 7].getValue());
            obj.setPosY(v[baseIndex + 8].getValue());
            obj.setCpx(v[baseIndex + 9].getValue());
            obj.setCpy(v[baseIndex + 10].getValue());
            obj.setCpd(v[baseIndex + 11].getValue());
            obj.setWidth(toDouble(v[baseIndex + 12]));
            obj.setLength(toDouble(v[baseIndex + 13]));
            obj.setHeading(toDouble(v[baseIndex + 14]));
            obj.setVxRel(toDouble(v[baseIndex + 15]));
            obj.setVxAbs(toDouble(v[baseIndex + 16]));
            obj.setVyRel(toDouble(v[baseIndex + 17]));
            obj.setVyAbs(toDouble(v[baseIndex + 18]));
            obj.setAxRel(toDouble(v[baseIndex + 19]));
            obj.setAxAbs(toDouble(v[baseIndex + 20]));
            obj.setAyRel(toDouble(v[baseIndex + 21]));
            obj.setAyAbs(toDouble(v[baseIndex + 22]));
            obj.setTimeOffset(toInt(v[baseIndex + 35]));
            
            if (isNumber(v[baseIndex + 36])) {
                obj.setColor(new Color(
                    (int)v[baseIndex + 36].getValue(),
                    (int)v[baseIndex + 37].getValue(),
                    (int)v[baseIndex + 38].getValue()
                ));
            }
            
            obj.setObjectClassConfidence(toDouble(v[baseIndex + 39]));
            obj.setPosXSigma(toDouble(v[baseIndex + 40]));
            obj.setPosYSigma(toDouble(v[baseIndex + 41]));

            int trajOffset = (int)v[baseIndex + 31].getValue();
            int trajSize = (int)v[baseIndex + 32].getValue();
            int contOffset = (int)v[baseIndex + 33].getValue();
            int contSize = (int)v[baseIndex + 34].getValue();

            if (v.length == sizeWithExtra) {
                FloatPoint[] trajectory = new FloatPoint[trajSize];
                for (int n = 0; n < trajSize; n++) {
                    trajectory[n] = new FloatPoint();
                    trajectory[n].setX((float)v[trajBase + 2 * trajOffset + 2 * n].getValue());
                    trajectory[n].setY((float)v[trajBase + 2 * trajOffset + 2 * n + 1].getValue());
                }
                obj.setTrajectory(trajectory);

                FloatPoint[] contour = new FloatPoint[contSize];
                for (int n = 0; n < contSize; n++) {
                    contour[n] = new FloatPoint();
                    contour[n].setX((float)v[contBase + 2 * contOffset + 2 * n].getValue());
                    contour[n].setY((float)v[contBase + 2 * contOffset + 2 * n + 1].getValue());
                }
                obj.setContour(contour);
            } else {
                obj.setTrajectory(new FloatPoint[0]);
                obj.setContour(new FloatPoint[0]);
            }

            objects.add(obj);
        }

        fovs.clear();
        for (int i = 0; i < nFovs; i++) {
            FovDescription fov = new FovDescription();
            int baseIndex = fovBase + 6 * i;

            fov.setPositionX(v[baseIndex].getValue());
            fov.setPositionY(v[baseIndex + 1].getValue());
            fov.setOrientation(v[baseIndex + 2].getValue());
            fov.setAngleRange(v[baseIndex + 3].getValue());
            fov.setDistanceRange(v[baseIndex + 4].getValue());
            fov.setBlindRange(v[baseIndex + 5].getValue());

            fovs.add(fov);
        }

        return true;
    }

    private boolean fromObjSensorSampleV6(GeneralSample sample) {
        GeneralElement[] v = sample.getValues();
        if (v.length < 16) return false;

        if (isNotNumber(v[0]) ||
            isNotNumber(v[1]) ||
            isNotNumber(v[2]) ||
            isNotNumber(v[3])) return false;

        int nObjects = (int)v[0].getValue();
        int nFovs = (int)v[1].getValue();
        int nTrajectories = (int)v[2].getValue();
        int nContours = (int)v[3].getValue();

        int sizeWithExtra = 16 + nObjects * 52 + nFovs * 6 + (nTrajectories + nContours) * 2;
        int sizeWithoutExtra = 16 + nObjects * 52 + nFovs * 6;
        if (v.length != sizeWithExtra && v.length != sizeWithoutExtra) return false;

        keyObjectFrontIndex = toInt(v[4]);
        keyObjectLeftIndex = toInt(v[5]);
        keyObjectRightIndex = toInt(v[6]);

        vehicleSpeed = toDouble(v[7]);
        vehicleCurvature = toDouble(v[8]);
        vehicleWidth = toDouble(v[9]);
        vehicleLength = toDouble(v[10]);
        vehicleWheelBase = toDouble(v[11]);
        vehicleFrontOverhang = toDouble(v[12]);

        int objBase = 16;
        int fovBase = objBase + 52 * nObjects;
        int trajBase = fovBase + 6 * nFovs;
        int contBase = trajBase + 2 * nTrajectories;

        objects.clear();
        for (int i = 0; i < nObjects; i++) {
            ObjectInfo obj = new ObjectInfo();
            int baseIndex = objBase + 52 * i;
            obj.setId((int)v[baseIndex + 0].getValue());
            obj.setAge((int)v[baseIndex + 1].getValue());
            obj.setRawId(toInt(v[baseIndex + 2]));
            obj.setRawAge(toInt(v[baseIndex + 3]));
            obj.setObjectRawClassId(toInt(v[baseIndex + 4]));
            obj.setObjectClass(ObjectClass.fromValue((int)v[baseIndex + 5].getValue()));
            obj.setPosMode(PositionMode.fromValue((int)v[baseIndex + 6].getValue()));
            obj.setPosX(v[baseIndex + 7].getValue());
            obj.setPosY(v[baseIndex + 8].getValue());
            obj.setCpx(v[baseIndex + 9].getValue());
            obj.setCpy(v[baseIndex + 10].getValue());
            obj.setCpd(v[baseIndex + 11].getValue());
            obj.setWidth(toDouble(v[baseIndex + 12]));
            obj.setLength(toDouble(v[baseIndex + 13]));
            obj.setHeading(toDouble(v[baseIndex + 14]));
            obj.setVxRel(toDouble(v[baseIndex + 15]));
            obj.setVxAbs(toDouble(v[baseIndex + 16]));
            obj.setVyRel(toDouble(v[baseIndex + 17]));
            obj.setVyAbs(toDouble(v[baseIndex + 18]));
            obj.setAxRel(toDouble(v[baseIndex + 19]));
            obj.setAxAbs(toDouble(v[baseIndex + 20]));
            obj.setAyRel(toDouble(v[baseIndex + 21]));
            obj.setAyAbs(toDouble(v[baseIndex + 22]));
            
            if (isNumber(v[baseIndex + 23])) {
                obj.setColor(new Color(
                    (int)v[baseIndex + 23].getValue(),
                    (int)v[baseIndex + 24].getValue(),
                    (int)v[baseIndex + 25].getValue()
                ));
            }
            
            obj.setTimeOffset(toInt(v[baseIndex + 26]));
            obj.setConfidence(toDouble(v[baseIndex + 27]));
            obj.setObjectClassConfidence(toDouble(v[baseIndex + 28]));
            obj.setHeight(toDouble(v[baseIndex + 29]));
            obj.setPosZ(toDouble(v[baseIndex + 30]));
            obj.setPosXSigma(toDouble(v[baseIndex + 31]));
            obj.setPosYSigma(toDouble(v[baseIndex + 32]));
            obj.setPosZSigma(toDouble(v[baseIndex + 33]));
            obj.setRcs(toDouble(v[baseIndex + 42]));
            obj.setVLng(toDouble(v[baseIndex + 43]));
            obj.setVLat(toDouble(v[baseIndex + 44]));
            obj.setYawRate(toDouble(v[baseIndex + 45]));
            obj.setCurvature(toDouble(v[baseIndex + 46]));

            int trajOffset = (int)v[baseIndex + 48].getValue();
            int trajSize = (int)v[baseIndex + 49].getValue();
            int contOffset = (int)v[baseIndex + 50].getValue();
            int contSize = (int)v[baseIndex + 51].getValue();

            if (v.length == sizeWithExtra) {
                FloatPoint[] trajectory = new FloatPoint[trajSize];
                for (int n = 0; n < trajSize; n++) {
                    trajectory[n] = new FloatPoint();
                    trajectory[n].setX((float)v[trajBase + 2 * trajOffset + 2 * n].getValue());
                    trajectory[n].setY((float)v[trajBase + 2 * trajOffset + 2 * n + 1].getValue());
                }
                obj.setTrajectory(trajectory);

                FloatPoint[] contour = new FloatPoint[contSize];
                for (int n = 0; n < contSize; n++) {
                    contour[n] = new FloatPoint();
                    contour[n].setX((float)v[contBase + 2 * contOffset + 2 * n].getValue());
                    contour[n].setY((float)v[contBase + 2 * contOffset + 2 * n + 1].getValue());
                }
                obj.setContour(contour);
            } else {
                obj.setTrajectory(new FloatPoint[0]);
                obj.setContour(new FloatPoint[0]);
            }

            objects.add(obj);
        }

        fovs.clear();
        for (int i = 0; i < nFovs; i++) {
            FovDescription fov = new FovDescription();
            int baseIndex = fovBase + 6 * i;

            fov.setPositionX(v[baseIndex].getValue());
            fov.setPositionY(v[baseIndex + 1].getValue());
            fov.setOrientation(v[baseIndex + 2].getValue());
            fov.setAngleRange(v[baseIndex + 3].getValue());
            fov.setDistanceRange(v[baseIndex + 4].getValue());
            fov.setBlindRange(v[baseIndex + 5].getValue());

            fovs.add(fov);
        }

        return true;
    }

    private boolean fromObjSensorSampleV7(GeneralSample sample) {
        GeneralElement[] v = sample.getValues();
        if (v.length < 20) return false;

        // 检查基本数值是否有效
        if (isNotNumber(v[0]) ||
            isNotNumber(v[1]) ||
            isNotNumber(v[2]) ||
            isNotNumber(v[3])) return false;

        int nObjects = (int)v[0].getValue();
        int nFovs = (int)v[1].getValue();
        int nTrajectories = (int)v[2].getValue();
        int nContours = (int)v[3].getValue();

        int sizeWithExtra = 20 + nObjects * 72 + nFovs * 6 + (nTrajectories + nContours) * 2;
        int sizeWithoutExtra = 20 + nObjects * 72 + nFovs * 6;
        if (v.length != sizeWithExtra && v.length != sizeWithoutExtra) return false;

        keyObjectFrontIndex = toInt(v[4]);
        keyObjectLeftIndex = toInt(v[5]);
        keyObjectRightIndex = toInt(v[6]);

        vehicleSpeed = toDouble(v[7]);
        vehicleCurvature = toDouble(v[8]);
        vehicleWidth = toDouble(v[9]);
        vehicleLength = toDouble(v[10]);
        vehicleWheelBase = toDouble(v[11]);
        vehicleFrontOverhang = toDouble(v[12]);

        int objBase = 20;
        int fovBase = objBase + 72 * nObjects;
        int trajBase = fovBase + 6 * nFovs;
        int contBase = trajBase + 2 * nTrajectories;

        objects.clear();
        for (int i = 0; i < nObjects; i++) {
            ObjectInfo obj = new ObjectInfo();
            int baseIndex = objBase + 72 * i;
            obj.setId((int)v[baseIndex + 0].getValue());
            obj.setAge((int)v[baseIndex + 1].getValue());
            obj.setRawId(toInt(v[baseIndex + 2]));
            obj.setRawAge(toInt(v[baseIndex + 3]));
            obj.setObjectRawClassId(toInt(v[baseIndex + 4]));
            obj.setObjectClass(ObjectClass.fromValue((int)v[baseIndex + 5].getValue()));
            obj.setPosMode(PositionMode.fromValue((int)v[baseIndex + 6].getValue()));
            obj.setPosX(v[baseIndex + 7].getValue());
            obj.setPosY(v[baseIndex + 8].getValue());
            obj.setPosZ(toDouble(v[baseIndex + 9]));
            obj.setPosXSigma(toDouble(v[baseIndex + 10]));
            obj.setPosYSigma(toDouble(v[baseIndex + 11]));
            obj.setPosZSigma(toDouble(v[baseIndex + 12]));
            obj.setCpx(v[baseIndex + 13].getValue());
            obj.setCpy(v[baseIndex + 14].getValue());
            obj.setCpd(v[baseIndex + 15].getValue());
            obj.setWidth(toDouble(v[baseIndex + 16]));
            obj.setLength(toDouble(v[baseIndex + 17]));
            obj.setHeight(toDouble(v[baseIndex + 18]));
            obj.setHeading(toDouble(v[baseIndex + 19]));
            obj.setVxRel(toDouble(v[baseIndex + 20]));
            obj.setVxAbs(toDouble(v[baseIndex + 21]));
            obj.setVyRel(toDouble(v[baseIndex + 22]));
            obj.setVyAbs(toDouble(v[baseIndex + 23]));
            obj.setVzRel(toDouble(v[baseIndex + 24]));
            obj.setVzAbs(toDouble(v[baseIndex + 25]));
            obj.setAxRel(toDouble(v[baseIndex + 26]));
            obj.setAxAbs(toDouble(v[baseIndex + 27]));
            obj.setAyRel(toDouble(v[baseIndex + 28]));
            obj.setAyAbs(toDouble(v[baseIndex + 29]));
            obj.setAzRel(toDouble(v[baseIndex + 30]));
            obj.setAzAbs(toDouble(v[baseIndex + 31]));
            
            if (isNumber(v[baseIndex + 32])) {
                obj.setColor(new Color(
                    (int)v[baseIndex + 32].getValue(),
                    (int)v[baseIndex + 33].getValue(),
                    (int)v[baseIndex + 34].getValue()
                ));
            }
            
            obj.setTimeOffset(toInt(v[baseIndex + 35]));
            obj.setConfidence(toDouble(v[baseIndex + 36]));
            obj.setObjectClassConfidence(toDouble(v[baseIndex + 37]));
            obj.setRcs(toDouble(v[baseIndex + 46]));
            obj.setVLng(toDouble(v[baseIndex + 47]));
            obj.setVLat(toDouble(v[baseIndex + 48]));
            obj.setYawRate(toDouble(v[baseIndex + 49]));
            obj.setCurvature(toDouble(v[baseIndex + 50]));

            int trajOffset = (int)v[baseIndex + 68].getValue();
            int trajSize = (int)v[baseIndex + 69].getValue();
            int contOffset = (int)v[baseIndex + 70].getValue();
            int contSize = (int)v[baseIndex + 71].getValue();

            if (v.length == sizeWithExtra) {
                FloatPoint[] trajectory = new FloatPoint[trajSize];
                for (int n = 0; n < trajSize; n++) {
                    trajectory[n] = new FloatPoint();
                    trajectory[n].setX((float)v[trajBase + 2 * trajOffset + 2 * n].getValue());
                    trajectory[n].setY((float)v[trajBase + 2 * trajOffset + 2 * n + 1].getValue());
                }
                obj.setTrajectory(trajectory);

                FloatPoint[] contour = new FloatPoint[contSize];
                for (int n = 0; n < contSize; n++) {
                    contour[n] = new FloatPoint();
                    contour[n].setX((float)v[contBase + 2 * contOffset + 2 * n].getValue());
                    contour[n].setY((float)v[contBase + 2 * contOffset + 2 * n + 1].getValue());
                }
                obj.setContour(contour);
            } else {
                obj.setTrajectory(new FloatPoint[0]);
                obj.setContour(new FloatPoint[0]);
            }

            objects.add(obj);
        }

        fovs.clear();
        for (int i = 0; i < nFovs; i++) {
            FovDescription fov = new FovDescription();
            int baseIndex = fovBase + 6 * i;

            fov.setPositionX(v[baseIndex].getValue());
            fov.setPositionY(v[baseIndex + 1].getValue());
            fov.setOrientation(v[baseIndex + 2].getValue());
            fov.setAngleRange(v[baseIndex + 3].getValue());
            fov.setDistanceRange(v[baseIndex + 4].getValue());
            fov.setBlindRange(v[baseIndex + 5].getValue());

            fovs.add(fov);
        }

        return true;
    }
    
    // 工具方法，判断GeneralElement是否不是数字
    private boolean isNotNumber(GeneralElement element) {
        return element == null || !isNumber(element);
    }
    
    // 工具方法，判断GeneralElement是否是数字
    private boolean isNumber(GeneralElement element) {
        return element != null && element.isValid() && !element.isText();
    }
    
    // 工具方法，从GeneralElement获取Integer或null
    private Integer toInt(GeneralElement element) {
        if (element == null || !isNumber(element)) {
            return null;
        }
        return (int)element.getValue();
    }
    
    // 工具方法，从GeneralElement获取Double或null
    private Double toDouble(GeneralElement element) {
        if (element == null || !isNumber(element)) {
            return null;
        }
        return element.getValue();
    }
    
    /**
     * 是否支持插值
     */
    @Override
    public boolean supportInterpolation() {
        return true;
    }
    
    /**
     * 对两个目标物传感器样本进行插值
     */
    @Override
    public Sample interpolate(SearchResult result) {
        ObjectSensorSample s1 = (ObjectSensorSample)result.getS1();
        ObjectSensorSample s2 = (ObjectSensorSample)result.getS2();
        double w1 = result.getW1();
        double w2 = result.getW2();
        
        ObjectSensorSample output = new ObjectSensorSample();
        
        // 设置车辆和FOV参数
        output.vehicleWidth = s1.vehicleWidth;
        output.vehicleLength = s1.vehicleLength;
        output.vehicleSpeed = s1.vehicleSpeed * w1 + s2.vehicleSpeed * w2;
        output.vehicleCurvature = s1.vehicleCurvature * w1 + s2.vehicleCurvature * w2;
        output.vehicleWheelBase = s1.vehicleWheelBase;
        output.vehicleFrontOverhang = s1.vehicleFrontOverhang;
        
        // 复制FOV列表
        output.fovs = new ArrayList<>(s1.fovs);
        
        // 处理关键目标ID匹配
        Integer koFrontID = null, koLeftID = null, koRightID = null;
        if (s1.keyObjectFrontIndex != null && s2.keyObjectFrontIndex != null &&
            s1.objects.get(s1.keyObjectFrontIndex).getId() == s2.objects.get(s2.keyObjectFrontIndex).getId()) {
            koFrontID = s1.objects.get(s1.keyObjectFrontIndex).getId();
        }
        if (s1.keyObjectLeftIndex != null && s2.keyObjectLeftIndex != null &&
            s1.objects.get(s1.keyObjectLeftIndex).getId() == s2.objects.get(s2.keyObjectLeftIndex).getId()) {
            koLeftID = s1.objects.get(s1.keyObjectLeftIndex).getId();
        }
        if (s1.keyObjectRightIndex != null && s2.keyObjectRightIndex != null &&
            s1.objects.get(s1.keyObjectRightIndex).getId() == s2.objects.get(s2.keyObjectRightIndex).getId()) {
            koRightID = s1.objects.get(s1.keyObjectRightIndex).getId();
        }
        
        // 创建目标物列表并进行插值
        output.objects = new ArrayList<>();
        int index = 0;
        for (ObjectInfo o1 : s1.objects) {
            int id = o1.getId();
            for (ObjectInfo o2 : s2.objects) {
                if (o2.getId() != id) continue;
                
                // 设置关键目标索引
                if (koFrontID != null && koFrontID == id) output.keyObjectFrontIndex = index;
                if (koLeftID != null && koLeftID == id) output.keyObjectLeftIndex = index;
                if (koRightID != null && koRightID == id) output.keyObjectRightIndex = index;
                
                // 计算插值权重（考虑时间偏移）
                double ow1 = w1, ow2 = w2;
                if (o1.getTimeOffset() != null || o2.getTimeOffset() != null) {
                    double ot1 = s1.getOffset();
                    if (o1.getTimeOffset() != null) ot1 += 0.000001 * o1.getTimeOffset();
                    double ot2 = s2.getOffset();
                    if (o2.getTimeOffset() != null) ot2 += 0.000001 * o2.getTimeOffset();
                    if (ot2 - ot1 >= 0.000001) {
                        double targetTime = s1.getOffset() + (s2.getOffset() - s1.getOffset()) * w2;
                        ow1 = (ot2 - targetTime) / (ot2 - ot1);
                        ow2 = (targetTime - ot1) / (ot2 - ot1);
                    }
                }
                
                // 创建插值后的目标物对象
                ObjectInfo newObj = new ObjectInfo();
                newObj.setId(id);
                newObj.setAge(o1.getAge());
                newObj.setRawId(o1.getRawId());
                newObj.setRawAge(o1.getRawAge());
                
                // 基本属性插值
                if (o1.getConfidence() != null && o2.getConfidence() != null) {
                    newObj.setConfidence(o1.getConfidence() * ow1 + o2.getConfidence() * ow2);
                }
                newObj.setColor(ow1 > ow2 ? o1.getColor() : o2.getColor());
                newObj.setObjectClass(ow1 > ow2 ? o1.getObjectClass() : o2.getObjectClass());
                newObj.setObjectRawClassId(ow1 > ow2 ? o1.getObjectRawClassId() : o2.getObjectRawClassId());
                
                // 置信度插值
                if (o1.getObjectClassConfidence() != null && o2.getObjectClassConfidence() != null) {
                    newObj.setObjectClassConfidence(o1.getObjectClassConfidence() * ow1 + o2.getObjectClassConfidence() * ow2);
                }
                
                // 位置属性
                newObj.setPosMode(o1.getPosMode());
                newObj.setPosX(o1.getPosX() * ow1 + o2.getPosX() * ow2);
                newObj.setPosY(o1.getPosY() * ow1 + o2.getPosY() * ow2);
                
                // Z轴位置插值
                if (o1.getPosZ() != null && o2.getPosZ() != null) {
                    newObj.setPosZ(o1.getPosZ() * ow1 + o2.getPosZ() * ow2);
                }
                
                // 位置精度插值
                if (o1.getPosXSigma() != null && o2.getPosXSigma() != null) {
                    newObj.setPosXSigma(o1.getPosXSigma() * ow1 + o2.getPosXSigma() * ow2);
                }
                if (o1.getPosYSigma() != null && o2.getPosYSigma() != null) {
                    newObj.setPosYSigma(o1.getPosYSigma() * ow1 + o2.getPosYSigma() * ow2);
                }
                if (o1.getPosZSigma() != null && o2.getPosZSigma() != null) {
                    newObj.setPosZSigma(o1.getPosZSigma() * ow1 + o2.getPosZSigma() * ow2);
                }
                
                // 最近点
                newObj.setCpx(o1.getCpx() * ow1 + o2.getCpx() * ow2);
                newObj.setCpy(o1.getCpy() * ow1 + o2.getCpy() * ow2);
                newObj.setCpd(o1.getCpd() * ow1 + o2.getCpd() * ow2);
                
                // 速度插值
                if (o1.getVxAbs() != null && o2.getVxAbs() != null) {
                    newObj.setVxAbs(o1.getVxAbs() * ow1 + o2.getVxAbs() * ow2);
                }
                if (o1.getVyAbs() != null && o2.getVyAbs() != null) {
                    newObj.setVyAbs(o1.getVyAbs() * ow1 + o2.getVyAbs() * ow2);
                }
                if (o1.getVzAbs() != null && o2.getVzAbs() != null) {
                    newObj.setVzAbs(o1.getVzAbs() * ow1 + o2.getVzAbs() * ow2);
                }
                
                // 加速度插值
                if (o1.getAxAbs() != null && o2.getAxAbs() != null) {
                    newObj.setAxAbs(o1.getAxAbs() * ow1 + o2.getAxAbs() * ow2);
                }
                if (o1.getAyAbs() != null && o2.getAyAbs() != null) {
                    newObj.setAyAbs(o1.getAyAbs() * ow1 + o2.getAyAbs() * ow2);
                }
                if (o1.getAzAbs() != null && o2.getAzAbs() != null) {
                    newObj.setAzAbs(o1.getAzAbs() * ow1 + o2.getAzAbs() * ow2);
                }
                
                // 相对速度插值
                if (o1.getVxRel() != null && o2.getVxRel() != null) {
                    newObj.setVxRel(o1.getVxRel() * ow1 + o2.getVxRel() * ow2);
                }
                if (o1.getVyRel() != null && o2.getVyRel() != null) {
                    newObj.setVyRel(o1.getVyRel() * ow1 + o2.getVyRel() * ow2);
                }
                if (o1.getVzRel() != null && o2.getVzRel() != null) {
                    newObj.setVzRel(o1.getVzRel() * ow1 + o2.getVzRel() * ow2);
                }
                
                // 相对加速度插值
                if (o1.getAxRel() != null && o2.getAxRel() != null) {
                    newObj.setAxRel(o1.getAxRel() * ow1 + o2.getAxRel() * ow2);
                }
                if (o1.getAyRel() != null && o2.getAyRel() != null) {
                    newObj.setAyRel(o1.getAyRel() * ow1 + o2.getAyRel() * ow2);
                }
                if (o1.getAzRel() != null && o2.getAzRel() != null) {
                    newObj.setAzRel(o1.getAzRel() * ow1 + o2.getAzRel() * ow2);
                }
                
                // 雷达参数插值
                if (o1.getRcs() != null && o2.getRcs() != null) {
                    newObj.setRcs(o1.getRcs() * ow1 + o2.getRcs() * ow2);
                }
                if (o1.getVLng() != null && o2.getVLng() != null) {
                    newObj.setVLng(o1.getVLng() * ow1 + o2.getVLng() * ow2);
                }
                if (o1.getVLat() != null && o2.getVLat() != null) {
                    newObj.setVLat(o1.getVLat() * ow1 + o2.getVLat() * ow2);
                }
                
                // 运动学参数插值
                if (o1.getYawRate() != null && o2.getYawRate() != null) {
                    newObj.setYawRate(o1.getYawRate() * ow1 + o2.getYawRate() * ow2);
                }
                if (o1.getCurvature() != null && o2.getCurvature() != null) {
                    newObj.setCurvature(o1.getCurvature() * ow1 + o2.getCurvature() * ow2);
                }
                
                // 角度特殊插值
                newObj.setHeading(interpolateAngle(o1.getHeading(), o2.getHeading(), ow1, ow2));
                
                // 尺寸插值
                if (o1.getWidth() != null && o2.getWidth() != null) {
                    newObj.setWidth(o1.getWidth() * ow1 + o2.getWidth() * ow2);
                }
                if (o1.getLength() != null && o2.getLength() != null) {
                    newObj.setLength(o1.getLength() * ow1 + o2.getLength() * ow2);
                }
                if (o1.getHeight() != null && o2.getHeight() != null) {
                    newObj.setHeight(o1.getHeight() * ow1 + o2.getHeight() * ow2);
                }
                
                // 轨迹和轮廓取权重大的
                newObj.setTrajectory(ow1 > ow2 ? o1.getTrajectory() : o2.getTrajectory());
                newObj.setContour(ow1 > ow2 ? o1.getContour() : o2.getContour());
                
                output.objects.add(newObj);
                index++;
            }
        }

        return output;
    }
    
    /**
     * 对角度进行插值，使用正弦和余弦计算
     */
    private Double interpolateAngle(Double a1, Double a2, double w1, double w2) {
        if (a1 == null || a2 == null) return null;
        
        double deg2rad = Math.PI / 180;
        double x1 = Math.cos(a1 * deg2rad);
        double y1 = Math.sin(a1 * deg2rad);
        double x2 = Math.cos(a2 * deg2rad);
        double y2 = Math.sin(a2 * deg2rad);
        
        double xo = x1 * w1 + x2 * w2;
        double yo = y1 * w1 + y2 * w2;
        if (xo == 0 && yo == 0) return null;
        
        return Math.atan2(yo, xo) / deg2rad;
    }

    /**
     * 计算一阶TTC [s]
     */
    public Double getTTC1(ObjectInfo obj) {
        if (obj.getVxRel() != null) {
            double vxRelM = obj.getVxRel() / 3.6;

            if (obj.getCpx() > 0) {
                if (vxRelM >= -0.1) return 20.0;
                else return Math.min(20.0, -(obj.getCpx() / vxRelM));
            } else if (vehicleLength != null && obj.getCpx() < -vehicleLength) {
                if (vxRelM <= 0.1) return 20.0;
                else return Math.min(20.0, (-vehicleLength - obj.getCpx()) / vxRelM);
            } else return null;
        } else return null;
    }
    
    /**
     * 计算二阶TTC [s]
     */
    public Double getTTC2(ObjectInfo obj) {
        if (obj.getVxRel() != null && obj.getAxRel() != null) {
            double vxRelM = obj.getVxRel() / 3.6;
            double axRel = obj.getAxRel();

            if (obj.getCpx() > 0) {
                if (vxRelM >= -0.1) return 20.0;
                else if (Math.abs(axRel) < 0.1) return Math.min(20.0, -(obj.getCpx() / vxRelM));
                else if (vxRelM * vxRelM - 2 * axRel * obj.getCpx() >= 0) 
                    return Math.min(20.0, (-vxRelM - Math.sqrt(vxRelM * vxRelM - 2 * axRel * obj.getCpx())) / axRel); //0.5at^2 + vt - d = 0
                else return 20.0;
            } else if (vehicleLength != null && obj.getCpx() < -vehicleLength) {
                double dist = -vehicleLength - obj.getCpx();

                if (vxRelM <= 0.1) return 20.0;
                else if (Math.abs(axRel) < 0.1) return Math.min(20.0, (-vehicleLength - obj.getCpx()) / vxRelM);
                else if (vxRelM * vxRelM + 2 * axRel * dist >= 0) 
                    return Math.min(20.0, (-vxRelM + Math.sqrt(vxRelM * vxRelM + 2 * axRel * dist)) / axRel);
                return 20.0;
            } else return null;
        } else return null;
    }
    
    /**
     * 计算车间时距 [s]
     */
    public Double getTHW(ObjectInfo obj) {
        if (obj.getCpx() > 0 && vehicleSpeed != null) {
            double vxEgoM = vehicleSpeed / 3.6;
            if (vxEgoM <= 0.1) return 20.0;
            else return Math.min(20.0, obj.getCpx() / vxEgoM);
        } else return null;
    }
    
    /**
     * 计算避免碰撞最小减速度 [m/s²]
     * reactionTime=0: ODCA, >0: PDCA
     */
    public Double getDCA(ObjectInfo obj, double reactionTime) {
        if (obj.getCpx() > 0 && vehicleSpeed != null && obj.getVxAbs() != null && obj.getAxAbs() != null) {
            double vxEgoM = vehicleSpeed / 3.6;
            double vxAbsM = obj.getVxAbs() / 3.6;
            double axAbs = obj.getAxAbs();

            double egoDxReaction = vxEgoM * reactionTime;
            double objDxReaction = vxAbsM * reactionTime + 0.5 * axAbs * reactionTime * reactionTime;

            double dxRelAR = obj.getCpx() - 0.1 + objDxReaction - egoDxReaction;
            double egoVxAbsAR = vxEgoM;
            double objVxAbsAR = vxAbsM + axAbs * reactionTime;
            double objVxRelAR = vxAbsM - vxEgoM;
            double objAxAbsAR = axAbs;

            if (dxRelAR <= 0) return -20.0;

            if (objAxAbsAR >= 0) {
                if (objVxRelAR >= 0) return 0.0;
                else return Math.max(-20.0, objAxAbsAR - objVxRelAR * objVxRelAR / (2 * dxRelAR));
            } else {
                if (objVxRelAR >= 0) 
                    return Math.max(-20.0, egoVxAbsAR * egoVxAbsAR / (objVxAbsAR * objVxAbsAR / objAxAbsAR - 2 * dxRelAR));
                if (objVxRelAR * objVxRelAR / (2 * dxRelAR) < objAxAbsAR) 
                    return Math.max(-20.0, egoVxAbsAR * egoVxAbsAR / (objVxAbsAR * objVxAbsAR / objAxAbsAR - 2 * dxRelAR));
                else 
                    return Math.max(-20.0, objAxAbsAR - objVxRelAR * objVxRelAR / (2 * dxRelAR));
            }
        } else return null;
    }
    
    /**
     * 计算重叠率 [%]
     */
    public Double getOverlap(ObjectInfo obj, double boxCornerRatio) {
        if (vehicleCurvature != null && vehicleWidth != null) {
            double widthEgo = vehicleWidth;
            double curvEgo = vehicleCurvature;
            double fo = vehicleFrontOverhang == null ? 0.9 : vehicleFrontOverhang;
            double wb = vehicleWheelBase == null ? 2.8 : vehicleWheelBase;
            
            if (obj.getPosX() <= 0 || widthEgo <= 0) return null;

            double widthObj;
            if (obj.getPosMode() == PositionMode.CLOSEST_POINT) {
                switch (obj.getObjectCategory()) {
                    case CAR:
                        widthObj = 1.9;
                        break;
                    case PEDESTRIAN:
                        widthObj = 0.5;
                        break;
                    case TRUCK:
                        widthObj = 2.1;
                        break;
                    case BIKE:
                        widthObj = 0.6;
                        break;
                    case RAIL_CAR:
                        widthObj = 2.6;
                        break;
                    default:
                        widthObj = 1.0;
                        break;
                }
            } else { // BOX_CENTER
                FloatPoint[] corners = calcBoxCorners(obj, boxCornerRatio);
                double miny = Double.POSITIVE_INFINITY, maxy = Double.NEGATIVE_INFINITY;
                for (int i = 0; i < corners.length; i++) {
                    miny = Math.min(miny, corners[i].getY());
                    maxy = Math.max(maxy, corners[i].getY());
                }
                widthObj = maxy - miny;
            }

            double dyEgo = 0.5 * curvEgo * obj.getPosX() * obj.getPosX() + curvEgo * (fo + wb) * obj.getPosX();
            double dyObj = obj.getPosY();
            double overLap = (Math.min(dyEgo + 0.5 * widthEgo, dyObj + 0.5 * widthObj) - Math.max(dyEgo - 0.5 * widthEgo, dyObj - 0.5 * widthObj));
            double overLapRate = overLap / (overLap >= 0 ? Math.min(widthEgo, widthObj) : widthEgo);
            return (double)(int)(Math.max(-3.0, overLapRate) * 100);
        } else return null;
    }
    
    /**
     * 计算横向间距 [m]
     */
    public Double getLC(ObjectInfo obj, double boxCornerRatio) {
        if (vehicleWidth != null) {
            if (obj.getPosMode() == PositionMode.CLOSEST_POINT) {
                if (obj.getPosX() > 0) return null;
                else if (obj.getPosY() > 0) return obj.getPosY() - vehicleWidth * 0.5;
                else return -vehicleWidth * 0.5 - obj.getPosY();
            } else { // BOX_CENTER
                FloatPoint[] points = calcBoxCorners(obj, boxCornerRatio);

                double minx = Double.POSITIVE_INFINITY;
                double miny = Double.POSITIVE_INFINITY, maxy = Double.NEGATIVE_INFINITY;
                for (int i = 0; i < points.length; i++) {
                    FloatPoint pt = points[i];
                    if (pt.getX() < minx) minx = pt.getX();
                    if (pt.getY() < miny) miny = pt.getY();
                    if (pt.getY() > maxy) maxy = pt.getY();
                }

                if (minx > 0) return null;
                else if (miny > 0) return miny - vehicleWidth * 0.5;
                else if (maxy < 0) return -vehicleWidth * 0.5 - maxy;
                else return null;
            }
        } else return null;
    }
    
    /**
     * 计算后向间距 [m]
     */
    public Double getRC(ObjectInfo obj, double boxCornerRatio) {
        if (vehicleLength != null) {
            FloatPoint[] pts;
            if (obj.getPosMode() == PositionMode.CLOSEST_POINT) {
                pts = new FloatPoint[1];
                pts[0] = new FloatPoint();
                pts[0].setX((float)obj.getPosX());
                pts[0].setY((float)obj.getPosY());
            } else {
                pts = calcBoxCorners(obj, boxCornerRatio);
            }

            double maxx = Double.NEGATIVE_INFINITY;
            for (int i = 0; i < pts.length; i++) {
                FloatPoint pt = pts[i];
                if (pt.getX() > maxx) maxx = pt.getX();
            }

            if (maxx > 0) return null;
            else return -vehicleLength - maxx;
        } else return null;
    }
    
    /**
     * 计算目标物框的四个角点
     * 8个点顺序连接，k为斜方向点缩小比例(一般为0.9~1)
     */
    public static FloatPoint[] calcBoxCorners(ObjectInfo obj, double k) {
        if (obj.getWidth() == null || obj.getLength() == null) {
            return new FloatPoint[0];
        }
        
        double posx = obj.getPosX();
        double posy = obj.getPosY();
        double width = obj.getWidth();
        double length = obj.getLength();
        double heading = obj.getHeading() != null ? obj.getHeading() : 0;
        float coef = (float)k;

        FloatPoint[] pts = new FloatPoint[8];
        for (int i = 0; i < 8; i++) {
            pts[i] = new FloatPoint();
        }
        
        pts[0].setX((float)(length / 2)); 
        pts[0].setY(0);
        pts[1].setX((float)(length / 2) * coef); 
        pts[1].setY((float)(width / 2) * coef);
        pts[2].setX(0); 
        pts[2].setY((float)(width / 2));
        pts[3].setX((float)(-length / 2) * coef); 
        pts[3].setY((float)(width / 2) * coef);
        pts[4].setX((float)(-length / 2)); 
        pts[4].setY(0);
        pts[5].setX((float)(-length / 2) * coef); 
        pts[5].setY((float)(-width / 2) * coef);
        pts[6].setX(0); 
        pts[6].setY((float)(-width / 2));
        pts[7].setX((float)(length / 2) * coef); 
        pts[7].setY((float)(-width / 2) * coef);

        double deg2rad = Math.PI / 180;
        double coshd = Math.cos(heading * deg2rad);
        double sinhd = Math.sin(heading * deg2rad);
        for (int i = 0; i < 8; i++) {
            FloatPoint pt = pts[i];
            double x = coshd * pt.getX() - sinhd * pt.getY();
            double y = sinhd * pt.getX() + coshd * pt.getY();
            pts[i].setX((float)(posx + x));
            pts[i].setY((float)(posy + y));
        }
        return pts;
    }
    
    // 协议相关常量
    public static final String TARGET_PROTOCOL = "obj-sensor-sample-v7";
    public static final String[] PROTOCOLS = {
        "obj-sensor-sample-v3", "obj-sensor-sample-v4", "obj-sensor-sample-v5", 
        "obj-sensor-sample-v6", "obj-sensor-sample-v7"
    };
}